using System;
using System.Collections.Generic;
using System.Text;
using System.Drawing;
using LocalizationTest.Tools;

namespace LocalizationTest
{
  public class WorldStatePose2D : WorldState
  {
    private Pose2D pose;

    public Pose2D Pose
    {
      get { return pose; }
      set { pose = value; }
    }
    
    public WorldStatePose2D() 
    {
      pose = new Pose2D();
    }

    public WorldStatePose2D(double _positionX, double _positionY, double rotation)
    {
      this.pose = new Pose2D(_positionX, _positionY, rotation);
    }

    public WorldStatePose2D(Pose2D pose)
    {
      this.pose = pose;
    }

    public override object Clone()
    {
      return this.MemberwiseClone();
    }

    public override void Move(WorldState relativeState)
    {
      pose += (relativeState as WorldStatePose2D).pose;
    }

    public override void Kidnap()
    {
      pose = new Pose2D();
    }

    public override void DrawShaded(double shade, Color color, string drawingID)
    {
      Color color2 = DrawingTools.GetShadedColor(shade, ref color);
      DebugDrawingTool.CreateArrow(drawingID, 
        new Vector2(pose.translation.x, pose.translation.y),
        new Vector2(pose.translation.x + Math.Cos(pose.rotation) * 12, pose.translation.y + Math.Sin(pose.rotation) * 12),
        3, color2);
    }

    public override void Draw(double probability, string drawingID)
    {
      int value = (int)((1.0 - probability * 10.0) * 255.0);
      //      int value = (int)((probability * 50) * 255.0);
      if (value < 0) value = 0;
      if (value > 255) value = 255;
      Color color = Color.FromArgb(value, value, value);
      DebugDrawingTool.CreateLine(drawingID,
        new Vector2(pose.translation.x - Globals.rasterWidth / 2.0, pose.translation.y),
        new Vector2(pose.translation.x + Globals.rasterWidth / 2.0, pose.translation.y),
        Globals.rasterWidth, color);
    }

    public override void DrawWithIntensity(int intensity, string drawingID, double rasterWidth)
    {
      if (intensity < 0) intensity = 0;
      if (intensity > 255) intensity = 255;
      Color color = Color.FromArgb(intensity, intensity, intensity);
      DebugDrawingTool.CreateLine(drawingID,
        new Vector2(pose.translation.x - Globals.rasterWidth / 2.0, pose.translation.y),
        new Vector2(pose.translation.x + Globals.rasterWidth / 2.0, pose.translation.y),
        rasterWidth, color);
    }

    public override void DrawHighlighted(string drawingID, Color color)
    {
      DebugDrawingTool.CreateCircle(drawingID,
        new Vector2(pose.translation.x, pose.translation.y), 10, 3, color);
      DebugDrawingTool.CreateArrow(drawingID,
        new Vector2(pose.translation.x, pose.translation.y),
        new Vector2(
        pose.translation.x + 20 * Math.Cos(pose.rotation),
        pose.translation.y + 20 * Math.Sin(pose.rotation)), 3, color);
    }

    public override void DrawConnectingLine(WorldState otherWorldstate, string drawingID, Color color)
    {
      DebugDrawingTool.CreateLine(drawingID,
        new Vector2(pose.translation.x, pose.translation.y),
        new Vector2((otherWorldstate as WorldStatePose2D).pose.translation.x, (otherWorldstate as WorldStatePose2D).pose.translation.y), 3, color);
    }

    public override List<FunctionWorldState> CreateListOfWorldStates(WorldState minimum, WorldState maximum, WorldState step)
    {
      List<FunctionWorldState> function = new List<FunctionWorldState>();
      Pose2D bottomLeftCorner = (minimum as WorldStatePose2D).Pose;
      Pose2D topRightCorner = (maximum as WorldStatePose2D).Pose;
      Pose2D stepLength = (step as WorldStatePose2D).Pose;
      for (double x = bottomLeftCorner.translation.x; x <= topRightCorner.translation.x; x += stepLength.translation.x)
      {
        for (double y = bottomLeftCorner.translation.y; y <= topRightCorner.translation.y; y += stepLength.translation.y)
        {
          for (double angle = bottomLeftCorner.rotation; angle <= topRightCorner.rotation; angle += stepLength.rotation)
          {
            function.Add(new FunctionWorldState(new WorldStatePose2D(new Pose2D(x, y, angle)), 10));
          }
        }
      }
      return function;
    }

    public override string ToString()
    {
      return this.GetType().Name + "|" + pose.ToString();
    }

    public override WorldState Parse(string str)
    {
//      string[] parts = str.Split('|');
      return new WorldStatePose2D(Pose2D.Parse(str));
    }

    public override double[] ToDoubleArray()
    {
      double[] toReturn = new double[3];
      toReturn[0] = pose.translation.x;
      toReturn[1] = pose.translation.y;
      toReturn[2] = pose.rotation;
      return toReturn;
    }

    public override void FromDoubleArray(double[] doubleArray)
    {
      pose.translation.x = doubleArray[0];
      pose.translation.y = doubleArray[1];
      pose.rotation = doubleArray[2];
    }
  }
}
